#include "Time_Serve.h"

volatile uint16_t TIME_CNT = 0;
volatile uint16_t Buzzer_Flag = 0;

//调度表 循环变量
volatile uint8_t Flag_1ms = 0;
volatile uint8_t Flag_10ms = 0;
volatile uint8_t Flag_20ms = 0;
volatile uint8_t Flag_40ms = 0;
volatile uint8_t Flag_100ms = 0;

void TIMER_SCH_Init(void)
{
    NVIC_EnableIRQ(TIMER_SCH_INST_INT_IRQN);            //调度表定时器中断 使能 
}


//滴答定时器中断服务函数
void SysTick_Handler(void)//1ms
{
    static uint16_t Time_SysLED = 0;
    static uint16_t MPU_Time = 0;
    static uint16_t Buzzer_Time = 0;

    tick_ms ++;     //MPU精确延时计数器
    Time_SysLED ++;

    if(Buzzer_Flag){
        vBuzzer_Contrl(1);
        Buzzer_Time ++;
        if(Buzzer_Time == 300)//延时0.3s
        {
            Buzzer_Flag  = 0;
            Buzzer_Time = 0;
            vBuzzer_Contrl(0);
        }
    }
    
    if(Time_SysLED == 100){ Time_SysLED = 0; DL_GPIO_togglePins(LED_PORT, LED_Sys_PIN);}

    if(CReset_Flag == 0)
    {
        MPU_Time ++;
        if(MPU_Time == 8000){
            MPU_Time = 0;
            CReset_Flag = 1;
        }
    }

}


//调度定时器
void TIMER_SCH_INST_IRQHandler(void) 
{
    //如果产生了定时器中断
    switch( DL_TimerA_getPendingInterrupt(TIMER_SCH_INST) )
    {
        case DL_TIMER_IIDX_ZERO://如果是0溢出中断
            TIME_CNT ++;
            Flag_1ms = 1;
            if (TIME_CNT % 10 == 0) { Flag_10ms  = 1; }
            if (TIME_CNT % 20 == 0) { Flag_20ms  = 1; }

            if (TIME_CNT % 40 == 0) { 
                Flag_40ms  = 1; 
                Encoder_Caclulate();     //编码器计算
                Car_Data_Caclulate();    //小车数据计算
            }

            if (TIME_CNT % 100 == 0){ Flag_100ms = 1; }

            if (TIME_CNT >= 200){
                TIME_CNT = 0;
                DL_GPIO_togglePins(LED_PORT, LED_D1_PIN);
            }  
            break;

        default://其他的定时器中断
            break;
        //清除定时器中断标志
        NVIC_ClearPendingIRQ(TIMER_SCH_INST_INT_IRQN);
    }
}

//调度表
void TIME_SCH_Duty(void)
{
    if(Flag_1ms){
        vMPU_Calibration();
        Flag_1ms = 0;
    }

    if(Flag_10ms){
        Uart_Rx_Process();      //串口控制
        
        Flag_10ms = 0;
    }

    if(Flag_20ms){
        Read_Quad();            //陀螺仪数据
        //Sprintf("%.2f %.2f %.2f %.2f %.2f\r\n",CarData.Car_Speed,PIDw_Data.Target,PIDw_Data.Out,PIDw_Data.Kp,PIDw_Data.Ki);
        //Sprintf("%.2f %.2f %.2f %.2f %.2f\r\n",CarData.Car_Speed,PIDz_Data.Target,PIDz_Data.Out,PIDz_Data.Kp,PIDz_Data.Ki);
        Flag_20ms = 0;
    }

    if(Flag_40ms){
        //位置式PID闭环
        // PIDW_Calculate(&PIDw_Left_Motor,CarData.Left_Speed);   //左轮速度环
        // PIDW_Calculate(&PIDw_Right_Motor,CarData.Right_Speed); //右轮速度环

        // Motor_Control(PIDw_Left_Motor.Out,PIDw_Right_Motor.Out); 

        // PIDZ_Calculate(&PIDz_Left_Motor,CarData.Left_Speed);   //左轮速度环
        // PIDZ_Calculate(&PIDz_Right_Motor,CarData.Right_Speed); //右轮速度环
        // Motor_Control(PIDz_Left_Motor.Out, PIDz_Right_Motor.Out);
        Flag_40ms = 0;
    }    

    if(Flag_100ms){
        Time_Duty_100ms();//100ms

        Flag_100ms = 0;
    }  
}


void Time_Duty_100ms(void)
{
    // Sprintf("Pitch:%.2f Roll:%.2f Yaw:%.2f Flag:%d\r\n",
    // pitch, roll, YAW_Data, CReset_Flag);
    Sprintf("A:%d D:%d \r\n",Get_Encoder_CountA,Get_Encoder_CountD); 
    // Sprintf("A:%.2f D:%.2f \r\n",CarData.Right_Speed,CarData.Left_Speed);
    // Sprintf("%.2f %.2f\r\n",CarData.Right_Speed,CarData.Left_Speed);
    // Sprintf("%.2f %.2f %.2f\r\n",CarData.Car_Distance,CarData.Left_Speed,CarData.Right_Speed);
    // Sprintf("%d %d %.2f %.2f\r\n",
    // Get_Encoder_CountA,Get_Encoder_CountD,CarData.Left_Speed,CarData.Right_Speed);
    // Sprintf("%.2f %.2f %.2f\r\n",CarData.Car_Speed,PIDw_Data.Target,PIDw_Data.Out);
}


 

